import json
import os
import xml.etree.ElementTree as ET
import copy
from config import projection,XOSC_DEFAULT_PATH,XOSC_PATH,XOSC_RESULT_PATH,SIMU_PATH,EGO_PATH
from lxml import etree


# 是否逐帧输出主车轨迹（onsite格式场景默认不输出）
ego_flag = False
# 抽稀比例 （1：原始比例 10：10帧抽一帧）
bili = 1

# 配置主车信息，默认不用动
EgoInfo = {'type': ' 4', 'EgoId': "wanji-001", 'CarId': 871939}


def get_back_car_hsy(simu_path):
    result = {}
    data_temp = {}
    with open(simu_path, "r") as f:
        # K1_back_car_dict, K2_back_car_dict, K3_back_car_dict = {}, {}, {}
        # temp = [[K1_scene_area, K1_back_car_dict], [K2_scene_area, K2_back_car_dict], [K3_scene_area, K3_back_car_dict]]
        first_time = None
        while True:
            line = f.readline().strip()
            if line:
                data = json.loads(line.replace("'", '"'))
                car_list = data['value']
                for car in car_list:
                    # x, y = projection(car['longitude'], car['latitude'], inverse=False)

                    # print(car['longitude'], car['latitude'])
                    if car["vehicleType"] == 99:
                        continue

                    # print(car["vehicleType"])

                    # a = time.time()
                    x, y = projection(car['longitude'], car['latitude'], inverse=False)
                    # x, y = transform(wgs84_proj, projection, car['longitude'], car['latitude'])
                    # print(x,y)
                    # x, y = 0, 0
                    # print(time.time() - a)
                    if not first_time:
                        first_time = float(car['globalTimeStamp'])
                    if car["id"] not in result.keys():
                        # print("时间差", (float(car['globalTimeStamp']) - first_time) / 1000)

                        # 航向角、x, y, 帧数、类型
                        result[car["id"]] = [
                            [car["courseAngle"], x, y, (float(car['globalTimeStamp']) - first_time) / 1000,
                             car["vehicleType"]]]
                    else:
                        # print(car['id'])
                        # if car["id"] == 100001:
                        #     print((float(car['globalTimeStamp']) - first_time) / 1000)
                        result[car["id"]].append(
                            [car["courseAngle"], x, y, (float(car['globalTimeStamp']) - first_time) / 1000,
                             car["vehicleType"]])

            else:
                break

    return result


def get_back_car_tj(simu_path):
    result = {}
    data_temp = {}
    with open(simu_path, "r") as f:
        # K1_back_car_dict, K2_back_car_dict, K3_back_car_dict = {}, {}, {}
        # temp = [[K1_scene_area, K1_back_car_dict], [K2_scene_area, K2_back_car_dict], [K3_scene_area, K3_back_car_dict]]
        first_time = None
        while True:
            line = f.readline().strip()
            if line:
                data = json.loads(line)
                print(data)
                value = json.loads(data['value'])
                car_list = value['value']
                for car in car_list:
                    if car["vehicleType"] == 99:
                        continue
                    x, y = projection(car['longitude'], car['latitude'], inverse=False)
                    if not first_time:
                        first_time = float(car['globalTimeStamp'] / 1000)
                        print("first_time", first_time)
                    if car["id"] not in result.keys():
                        # print("时间差", (float(car['globalTimeStamp']) - first_time) / 1000)

                        # 航向角、x, y, 帧数、类型

                        result[car["id"]] = [
                            [car["courseAngle"], x, y, (float(car['globalTimeStamp'] / 1000) - first_time),
                             car["vehicleType"]]]
                    else:

                        result[car["id"]].append(
                            [car["courseAngle"], x, y, (float(car['globalTimeStamp'] / 1000) - first_time),
                             car["vehicleType"]])

            else:
                break

    return result


def pretty_print_xml(input_filename, output_filename, Ego):
    # 解析XML文件
    parser = etree.XMLParser(remove_comments=False)  # 可以移除空白的文本节点
    tree = etree.parse(input_filename, parser)
    root = tree.getroot()
    # 使用lxml的tostring方法，并设置pretty_print=True来保持缩进
    # encoding和xml_declaration参数可以根据需要调整
    output = etree.tostring(root, pretty_print=True, encoding='utf-8', xml_declaration=True, with_comments=True)
    output_str = output.decode()
    output_str = output_str.replace(
        '<Information entityRef="Ego"> </Information>',
        "<!--Information of the ego vehicle will be hidden, and its initial state and driving task will be explained in the comments below-->")
    output_str = output_str.replace(
        '<InitialState entityRef="Ego"> </InitialState>',
        "<!--[Initial State] v_init = " + str(Ego["initV"]) + ", x_init = " + str(Ego["initX"]) + " , y_init = " + str(
            Ego["initY"]) + " , heading_init = " + str(Ego["initH"]) + "-->")
    output_str = output_str.replace(
        '<DrivingTask entityRef="Ego"> </DrivingTask>',
        "<!--[Driving Task] x_target = (" + str(Ego["targetX1"]) + ", " + str(
            Ego["targetX2"]) + "), y_target = (" + str(
            Ego["targetY1"]) + ", " + str(Ego["targetY2"]) + ")-->")
    # print(output.decode())
    output_byte = output_str.encode()
    # # # 将字节串写入新文件
    # print("<!--[Initial State] v_init = 8.27770789530532, x_init = 972.568, y_init = 1001.892, heading_init = 6.153185307179586-->" in output_str)
    # print("output_filename",output_filename)
    # print("folder_path",folder_path.split("/"))
    with open(output_filename, 'wb') as f:
        f.write(output_byte)


def make_xosc(xosc_path, xosc_result_path, simu_path, Ego_path):
    global ego_flag, bili, EgoInfo
    t = get_back_car_hsy(simu_path)
    t['wanji-001'] = [[0.0, 0, 0, 0, 1], [0.0, 0, 0, 0, 1]]
    data = t
    # del t[100001]
    # 解析XML文件
    tree = ET.parse(XOSC_DEFAULT_PATH)
    root = tree.getroot()

    # 如果你知道特定的路径或结构，可以直接访问
    count = 0

    Entities = root.find('Entities')
    ScenarioObject = root.find('Entities').findall('ScenarioObject')[0]

    pedestrianScenarioObject = root.find('Entities').findall('ScenarioObject')[1]
    # TODO 车辆类型适配 7：行人，5：非机动车
    type_dict = {7: "pedestrian", 5: "bicycle"}
    # type_dict = {}

    for k, v in data.items():
        # print(k,v)
        # if len(v) < 10:
        #     continue
        type = v[0][-1]
        # print("类型", k,type, )
        if type == 7:
            ScenarioObject_copy = copy.deepcopy(pedestrianScenarioObject)
        else:
            ScenarioObject_copy = copy.deepcopy(ScenarioObject)
        if type in type_dict.keys():
            if type == 7:
                ScenarioObject_copy.find('Pedestrian').attrib['pedestrianCategory'] = type_dict[type]
            else:
                ScenarioObject_copy.find('Vehicle').attrib['vehicleCategory'] = type_dict[type]
                if type == 5:
                    # length="1.8404605390000000e+00" width="8.2371950100000002e-01"
                    ScenarioObject_copy.find('Vehicle').find('BoundingBox').find('Dimensions').attrib[
                        'length'] = "1.8404605390000000e+00"
                    ScenarioObject_copy.find('Vehicle').find('BoundingBox').find('Dimensions').attrib[
                        'width'] = "8.2371950100000002e-01"
        if str(k) == str(EgoInfo['EgoId']):
            print("EGO++++++")
            ScenarioObject_copy.attrib['name'] = "Ego"
        else:
            ScenarioObject_copy.attrib['name'] = str(k)
        Entities.append(ScenarioObject_copy)
    Entities.remove(ScenarioObject)
    Entities.remove(pedestrianScenarioObject)
    print("Private", root.find('Storyboard').find('Init').find('Actions').find('Private'))
    Story = root.find('Storyboard').find('Story')
    Act = root.find('Storyboard').find('Story').find('Act')
    import math

    for k, value in data.items():
        value = sorted(value, key=lambda x: float(x[3]))
        # if len(value) < 10:
        #     continue
        Act_copy = copy.deepcopy(Act)
        if str(k) == str(EgoInfo['EgoId']):
            print("EGO++++++")
            Act_copy.find('ManeuverGroup').attrib['name'] = 'Squence_Ego'
            Act_copy.attrib['name'] = 'Act_Ego'
            Act_copy.find('ManeuverGroup').find('Actors').find('EntityRef').attrib["entityRef"] = "Ego"
        else:
            Act_copy.find('ManeuverGroup').attrib['name'] = 'Squence_' + str(k)
            Act_copy.attrib['name'] = 'Act_' + str(k)
            Act_copy.find('ManeuverGroup').find('Actors').find('EntityRef').attrib["entityRef"] = str(k)
        Event_list = Act_copy.find('ManeuverGroup').find('Maneuver').findall('Event')

        for e in Event_list:
            if e.attrib['name'] == 'Event1':
                e.find('StartTrigger').find('ConditionGroup').find('Condition').attrib['delay'] = str(
                    round((value[0][3]), 1))
                Trajectory = e.find('Action').find('PrivateAction').find('RoutingAction').find(
                    'FollowTrajectoryAction').find('Trajectory')
                # print(Trajectory.attrib['name'])
                if str(k) == str(EgoInfo['EgoId']):
                    Trajectory.attrib['name'] = 'Trajectory_Ego'
                else:
                    Trajectory.attrib['name'] = 'Trajectory_' + str(k)
                Polyline = Trajectory.find('Shape').find('Polyline')
                Vertex = Trajectory.find('Shape').find('Polyline').find('Vertex')

                # print(k, value[0])
                count = 0
                # data_temp = []
                for v in range(len(value)):
                    if count % bili == 0:
                        Vertex_copy = copy.deepcopy(Vertex)

                        Vertex_copy.attrib['time'] = str(round((value[v][3]), 1))
                        Vertex_copy.find('Position').find('WorldPosition').attrib['x'] = str(value[v][1])
                        Vertex_copy.find('Position').find('WorldPosition').attrib['y'] = str(value[v][2])
                        Vertex_copy.find('Position').find('WorldPosition').attrib['h'] = str(
                            (-float(value[v][0]) + 90) * math.pi / 180)
                        Polyline.append(Vertex_copy)
                    count = count + 1
                # print(data_temp)
                Polyline.remove(Vertex)
            if e.attrib['name'] == 'AddEntityEvent':
                e.find('StartTrigger').find('ConditionGroup').find('Condition').attrib['delay'] = str(
                    round((value[0][3]), 1))
                if str(k) == str(EgoInfo['EgoId']):
                    Trajectory = e.find('Action').find('GlobalAction').find('EntityAction').attrib['entityRef'] = "Ego"
                else:
                    Trajectory = e.find('Action').find('GlobalAction').find('EntityAction').attrib['entityRef'] = str(k)

                Vertex_copy.find('Position').find('WorldPosition').attrib['x'] = str(value[v][1])
                Vertex_copy.find('Position').find('WorldPosition').attrib['y'] = str(value[v][2])
                Vertex_copy.find('Position').find('WorldPosition').attrib['h'] = str(
                    (-float(value[v][0]) + 90) * math.pi / 180)
                e.find('Action').find('GlobalAction').find('EntityAction').find('AddEntityAction').find(
                    'Position').find(
                    'WorldPosition').attrib['h'] = str((-float(value[0][0]) + 90) * math.pi / 180)

                e.find('Action').find('GlobalAction').find('EntityAction').find('AddEntityAction').find(
                    'Position').find(
                    'WorldPosition').attrib['x'] = str(value[0][1])
                e.find('Action').find('GlobalAction').find('EntityAction').find('AddEntityAction').find(
                    'Position').find(
                    'WorldPosition').attrib['y'] = str(value[0][2])

            if e.attrib['name'] == 'DeleteEntityEvent':
                if str(k) == str(EgoInfo['EgoId']):
                    e.find('Action').find('GlobalAction').find('EntityAction').attrib['entityRef'] = "Ego"
                    print("初始x,y,h", str(value[0][1]), str(value[0][2]),
                          str((-float(value[0][0]) + 90) * math.pi / 180))
                    print("结束x,y,h", str(value[-1][1]), str(value[-1][2]),
                          str((-float(value[-1][0]) + 90) * math.pi / 180))
                else:
                    e.find('Action').find('GlobalAction').find('EntityAction').attrib['entityRef'] = str(k)
                e.find('StartTrigger').find('ConditionGroup').find('Condition').attrib['delay'] = str(
                    round((value[-1][3]), 1))
        if str(k) == str(EgoInfo['EgoId']) and ego_flag:
            Story.append(Act_copy)
        if str(k) != str(EgoInfo['EgoId']):
            Story.append(Act_copy)

    Story.remove(Act)

    tree.write(xosc_path, encoding='utf-8')
    Ego = {}
    with open(Ego_path, 'r') as f:
        info = f.read()
        Ego = json.loads(info)

    initLon = Ego["initLon"]
    initLat = Ego["initLat"]
    targetLon1 = Ego["targetLon1"]
    targetLon2 = Ego["targetLon2"]
    targetLat1 = Ego["targetLat1"]
    targetLat2 = Ego["targetLat2"]

    initX, initY = projection(initLon, initLat, inverse=False)
    Ego["initX"] = initX
    Ego["initY"] = initY
    # print(targetLon1, )
    target_x1, target_y1 = projection(float(targetLon1), float(targetLat1), inverse=False)
    Ego["targetX1"] = target_x1
    Ego["targetY1"] = target_y1
    # target_x2, target_y2 = transform(wgs84_proj, projection, float(targetLon2), float(targetLat2))
    target_x2, target_y2 = projection(float(targetLon2), float(targetLat2), inverse=False)
    Ego["targetX2"] = target_x2
    Ego["targetY2"] = target_y2
    Ego['initV'] = 8.0
    Ego['initH'] = -float(Ego['initH'] + 90) * math.pi / 180
    print(Ego)
    pretty_print_xml(xosc_path, xosc_result_path, Ego)
    os.remove(xosc_path)


if __name__ == '__main__':
    make_xosc(XOSC_PATH, XOSC_RESULT_PATH, SIMU_PATH, EGO_PATH)

